﻿namespace JelloPhysics
{
    using Microsoft.Xna.Framework;
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.CompilerServices;
    using System.Runtime.InteropServices;

    public class World : IEnumerable<Body>, IEnumerable
    {
        protected List<Body> mBodies = new List<Body>();
        private List<BodyCollisionInfo> mCollisionList = new List<BodyCollisionInfo>();
        private MaterialPair mDefaultMatPair;
        private int mMaterialCount = 1;
        private MaterialPair[,] mMaterialPairs = new MaterialPair[1, 1];
        private int mPenetrationCount;
        private float mPenetrationThreshold;
        private Vector2 mWorldGridStep;
        private AABB mWorldLimits;
        private Vector2 mWorldSize;

        public World()
        {
            this.mDefaultMatPair.Friction = 0.3f;
            this.mDefaultMatPair.Elasticity = 0.8f;
            this.mDefaultMatPair.Collide = true;
            this.mDefaultMatPair.CollisionFilter = new collisionFilter(this.defaultCollisionFilter);
            this.mMaterialPairs[0, 0] = this.mDefaultMatPair;
            Vector2 min = new Vector2(-20f, -20f);
            Vector2 max = new Vector2(20f, 20f);
            this.setWorldLimits(min, max);
            this.mPenetrationThreshold = 0.3f;
        }

        private void _handleCollisions()
        {
            for (int i = 0; i < this.mCollisionList.Count; i++)
            {
                float num2;
                BodyCollisionInfo info = this.mCollisionList[i];
                PointMass mass = info.bodyA.getPointMass(info.bodyApm);
                PointMass mass2 = info.bodyB.getPointMass(info.bodyBpmA);
                PointMass mass3 = info.bodyB.getPointMass(info.bodyBpmB);
                Vector2 vector = new Vector2();
                vector.X = (mass2.Velocity.X + mass3.Velocity.X) * 0.5f;
                vector.Y = (mass2.Velocity.Y + mass3.Velocity.Y) * 0.5f;
                Vector2 vector2 = new Vector2();
                vector2.X = mass.Velocity.X - vector.X;
                vector2.Y = mass.Velocity.Y - vector.Y;
                Vector2.Dot(ref vector2, ref info.normal, out num2);
                if (this.mMaterialPairs[info.bodyA.Material, info.bodyB.Material].CollisionFilter(info.bodyA, info.bodyApm, info.bodyB, info.bodyBpmA, info.bodyBpmB, info.hitPt, num2))
                {
                    if (info.penetration > this.mPenetrationThreshold)
                    {
                        this.mPenetrationCount++;
                    }
                    else
                    {
                        float num7;
                        float num8;
                        float num15;
                        float num18;
                        float num3 = 1f - info.edgeD;
                        float edgeD = info.edgeD;
                        float f = (float.IsPositiveInfinity(mass2.Mass) || float.IsPositiveInfinity(mass3.Mass)) ? float.PositiveInfinity : (mass2.Mass + mass3.Mass);
                        float num6 = mass.Mass + f;
                        if (float.IsPositiveInfinity(mass.Mass))
                        {
                            num7 = 0f;
                            num8 = info.penetration + 0.001f;
                        }
                        else if (float.IsPositiveInfinity(f))
                        {
                            num7 = info.penetration + 0.001f;
                            num8 = 0f;
                        }
                        else
                        {
                            num7 = info.penetration * (f / num6);
                            num8 = info.penetration * (mass.Mass / num6);
                        }
                        float num9 = num8 * num3;
                        float num10 = num8 * edgeD;
                        float num11 = float.IsPositiveInfinity(mass.Mass) ? 0f : (1f / mass.Mass);
                        float num12 = float.IsPositiveInfinity(f) ? 0f : (1f / f);
                        float num13 = num11 + num12;
                        Vector2 vector3 = new Vector2();
                        float num14 = 1f + this.mMaterialPairs[info.bodyA.Material, info.bodyB.Material].Elasticity;
                        vector3.X = vector2.X * num14;
                        vector3.Y = vector2.Y * num14;
                        Vector2.Dot(ref vector3, ref info.normal, out num15);
                        num15 = -num15;
                        float num16 = num15 / num13;
                        if (!float.IsPositiveInfinity(mass.Mass))
                        {
                            mass.Position.X += info.normal.X * num7;
                            mass.Position.Y += info.normal.Y * num7;
                        }
                        if (!float.IsPositiveInfinity(mass2.Mass))
                        {
                            mass2.Position.X -= info.normal.X * num9;
                            mass2.Position.Y -= info.normal.Y * num9;
                        }
                        if (!float.IsPositiveInfinity(mass3.Mass))
                        {
                            mass3.Position.X -= info.normal.X * num10;
                            mass3.Position.Y -= info.normal.Y * num10;
                        }
                        Vector2 vOut = new Vector2();
                        VectorTools.getPerpendicular(ref info.normal, ref vOut);
                        float friction = this.mMaterialPairs[info.bodyA.Material, info.bodyB.Material].Friction;
                        Vector2.Dot(ref vector2, ref vOut, out num18);
                        num18 *= friction;
                        float num19 = num18 / num13;
                        if (num2 <= 0.0001f)
                        {
                            if (!float.IsPositiveInfinity(mass.Mass))
                            {
                                mass.Velocity.X += (info.normal.X * (num16 / mass.Mass)) - (vOut.X * (num19 / mass.Mass));
                                mass.Velocity.Y += (info.normal.Y * (num16 / mass.Mass)) - (vOut.Y * (num19 / mass.Mass));
                            }
                            if (!float.IsPositiveInfinity(f))
                            {
                                mass2.Velocity.X -= ((info.normal.X * (num16 / f)) * num3) - ((vOut.X * (num19 / f)) * num3);
                                mass2.Velocity.Y -= ((info.normal.Y * (num16 / f)) * num3) - ((vOut.Y * (num19 / f)) * num3);
                            }
                            if (!float.IsPositiveInfinity(f))
                            {
                                mass3.Velocity.X -= ((info.normal.X * (num16 / f)) * edgeD) - ((vOut.X * (num19 / f)) * edgeD);
                                mass3.Velocity.Y -= ((info.normal.Y * (num16 / f)) * edgeD) - ((vOut.Y * (num19 / f)) * edgeD);
                            }
                        }
                    }
                }
            }
            this.mCollisionList.Clear();
        }

        public void addBody(Body b)
        {
            if (!this.mBodies.Contains(b))
            {
                this.mBodies.Add(b);
            }
        }

        public int addMaterial()
        {
            MaterialPair[,] mMaterialPairs = this.mMaterialPairs;
            this.mMaterialCount++;
            this.mMaterialPairs = new MaterialPair[this.mMaterialCount, this.mMaterialCount];
            for (int i = 0; i < this.mMaterialCount; i++)
            {
                for (int j = 0; j < this.mMaterialCount; j++)
                {
                    if ((i < (this.mMaterialCount - 1)) && (j < (this.mMaterialCount - 1)))
                    {
                        this.mMaterialPairs[i, j] = mMaterialPairs[i, j];
                    }
                    else
                    {
                        this.mMaterialPairs[i, j] = this.mDefaultMatPair;
                    }
                }
            }
            return (this.mMaterialCount - 1);
        }

        private void bodyCollide(Body bA, Body bB, List<BodyCollisionInfo> infoList)
        {
            int pointMassCount = bA.PointMassCount;
            int num2 = bB.PointMassCount;
            AABB aabb = bB.getAABB();
            BodyCollisionInfo item = new BodyCollisionInfo();
            BodyCollisionInfo info2 = new BodyCollisionInfo();
            for (int i = 0; i < pointMassCount; i++)
            {
                Vector2 position = bA.getPointMass(i).Position;
                if (aabb.contains(ref position) && bB.contains(ref position))
                {
                    int index = (i > 0) ? (i - 1) : (pointMassCount - 1);
                    int num5 = (i < (pointMassCount - 1)) ? (i + 1) : 0;
                    Vector2 vector2 = bA.getPointMass(index).Position;
                    Vector2 vector3 = bA.getPointMass(num5).Position;
                    Vector2 vector4 = new Vector2();
                    vector4.X = position.X - vector2.X;
                    vector4.Y = position.Y - vector2.Y;
                    Vector2 vector5 = new Vector2();
                    vector5.X = vector3.X - position.X;
                    vector5.Y = vector3.Y - position.Y;
                    Vector2 v = new Vector2();
                    v.X = vector4.X + vector5.X;
                    v.Y = vector4.Y + vector5.Y;
                    VectorTools.makePerpendicular(ref v);
                    float num6 = 100000f;
                    float num7 = 100000f;
                    item.Clear();
                    item.bodyA = bA;
                    item.bodyApm = i;
                    item.bodyB = bB;
                    info2.Clear();
                    info2.bodyA = bA;
                    info2.bodyApm = i;
                    info2.bodyB = bB;
                    bool flag = false;
                    int num8 = 0;
                    int num9 = 1;
                    for (int j = 0; j < num2; j++)
                    {
                        num8 = j;
                        if (j < (num2 - 1))
                        {
                            num9 = j + 1;
                        }
                        else
                        {
                            num9 = 0;
                        }
                        Vector2 vector9 = bB.getPointMass(num8).Position;
                        Vector2 vector10 = bB.getPointMass(num9).Position;
                        float num12 = ((vector9.X - position.X) * (vector9.X - position.X)) + ((vector9.Y - position.Y) * (vector9.Y - position.Y));
                        float num13 = ((vector10.X - position.X) * (vector10.X - position.X)) + ((vector10.Y - position.Y) * (vector10.Y - position.Y));
                        if (((num12 <= num6) || (num12 <= num7)) || ((num13 <= num6) || (num13 <= num7)))
                        {
                            Vector2 vector7;
                            Vector2 vector8;
                            float num11;
                            float num15;
                            float num14 = bB.getClosestPointOnEdgeSquared(position, j, out vector7, out vector8, out num11);
                            Vector2.Dot(ref v, ref vector8, out num15);
                            if (num15 <= 0f)
                            {
                                if (num14 < num6)
                                {
                                    num6 = num14;
                                    item.bodyBpmA = num8;
                                    item.bodyBpmB = num9;
                                    item.edgeD = num11;
                                    item.hitPt = vector7;
                                    item.normal = vector8;
                                    item.penetration = num14;
                                    flag = true;
                                }
                            }
                            else if (num14 < num7)
                            {
                                num7 = num14;
                                info2.bodyBpmA = num8;
                                info2.bodyBpmB = num9;
                                info2.edgeD = num11;
                                info2.hitPt = vector7;
                                info2.normal = vector8;
                                info2.penetration = num14;
                            }
                        }
                    }
                    if ((flag && (num6 > this.mPenetrationThreshold)) && (num7 < num6))
                    {
                        info2.penetration = (float) Math.Sqrt((double) info2.penetration);
                        if ((info2.bodyBpmA != -1) && (info2.bodyBpmB != -1))
                        {
                            infoList.Add(info2);
                        }
                    }
                    else
                    {
                        item.penetration = (float) Math.Sqrt((double) item.penetration);
                        if ((item.bodyBpmA != -1) && (item.bodyBpmB != -1))
                        {
                            infoList.Add(item);
                        }
                    }
                }
            }
        }

        private bool defaultCollisionFilter(Body A, int Apm, Body B, int Bpm1, int Bpm2, Vector2 hitPt, float normSpeed)
        {
            return true;
        }

        public Body getBody(int index)
        {
            if ((index >= 0) && (index < this.mBodies.Count))
            {
                return this.mBodies[index];
            }
            return null;
        }

        public Body getBodyContaining(ref Vector2 pt)
        {
            for (int i = 0; i < this.mBodies.Count; i++)
            {
                if (this.mBodies[i].contains(ref pt))
                {
                    return this.mBodies[i];
                }
            }
            return null;
        }

        public void getClosestPointMass(Vector2 pt, out int bodyID, out int pmID)
        {
            bodyID = -1;
            pmID = -1;
            float num = 1000f;
            for (int i = 0; i < this.mBodies.Count; i++)
            {
                float dist = 0f;
                int num4 = this.mBodies[i].getClosestPointMass(pt, out dist);
                if (dist < num)
                {
                    num = dist;
                    bodyID = i;
                    pmID = num4;
                }
            }
        }

        public IEnumerator<Body> GetEnumerator()
        {
            return this.mBodies.Cast<Body>().GetEnumerator();
        }

        public void removeBody(Body b)
        {
            if (this.mBodies.Contains(b))
            {
                this.mBodies.Remove(b);
            }
        }

        public void setMaterialPairCollide(int a, int b, bool collide)
        {
            if (((a >= 0) && (a < this.mMaterialCount)) && ((b >= 0) && (b < this.mMaterialCount)))
            {
                this.mMaterialPairs[a, b].Collide = collide;
                this.mMaterialPairs[b, a].Collide = collide;
            }
        }

        public void setMaterialPairData(int a, int b, float friction, float elasticity)
        {
            if (((a >= 0) && (a < this.mMaterialCount)) && ((b >= 0) && (b < this.mMaterialCount)))
            {
                this.mMaterialPairs[a, b].Friction = friction;
                this.mMaterialPairs[a, b].Elasticity = elasticity;
                this.mMaterialPairs[b, a].Friction = friction;
                this.mMaterialPairs[b, a].Elasticity = elasticity;
            }
        }

        public void setMaterialPairFilterCallback(int a, int b, collisionFilter filter)
        {
            if (((a >= 0) && (a < this.mMaterialCount)) && ((b >= 0) && (b < this.mMaterialCount)))
            {
                MaterialPair pair1 = this.mMaterialPairs[a, b];
                pair1.CollisionFilter = (collisionFilter) Delegate.Combine(pair1.CollisionFilter, filter);
                MaterialPair pair2 = this.mMaterialPairs[b, a];
                pair2.CollisionFilter = (collisionFilter) Delegate.Combine(pair2.CollisionFilter, filter);
            }
        }

        public void setWorldLimits(Vector2 min, Vector2 max)
        {
            this.mWorldLimits = new AABB(ref min, ref max);
            this.mWorldSize = max - min;
            this.mWorldGridStep = (Vector2) (this.mWorldSize / 32f);
        }

        IEnumerator IEnumerable.GetEnumerator()
        {
            return this.GetEnumerator();
        }

        public void update(float elapsed)
        {
            this.mPenetrationCount = 0;
            for (int i = 0; i < this.mBodies.Count; i++)
            {
                this.mBodies[i].derivePositionAndAngle(elapsed);
                this.mBodies[i].accumulateExternalForces();
                this.mBodies[i].accumulateInternalForces();
            }
            for (int j = 0; j < this.mBodies.Count; j++)
            {
                this.mBodies[j].integrate(elapsed);
            }
            for (int k = 0; k < this.mBodies.Count; k++)
            {
                this.mBodies[k].updateAABB(elapsed, false);
                this.updateBodyBitmask(this.mBodies[k]);
            }
            for (int m = 0; m < this.mBodies.Count; m++)
            {
                for (int num5 = m + 1; num5 < this.mBodies.Count; num5++)
                {
                    if ((this.mMaterialPairs[this.mBodies[m].Material, this.mBodies[num5].Material].Collide && (!this.mBodies[m].IsStatic || !this.mBodies[num5].IsStatic)) && (((this.mBodies[m].mBitMaskX.mask & this.mBodies[num5].mBitMaskX.mask) != 0) || ((this.mBodies[m].mBitMaskY.mask & this.mBodies[num5].mBitMaskY.mask) != 0)))
                    {
                        AABB aabb = this.mBodies[m].getAABB();
                        AABB box = this.mBodies[num5].getAABB();
                        if (aabb.intersects(ref box))
                        {
                            this.bodyCollide(this.mBodies[m], this.mBodies[num5], this.mCollisionList);
                            this.bodyCollide(this.mBodies[num5], this.mBodies[m], this.mCollisionList);
                        }
                    }
                }
            }
            this._handleCollisions();
            for (int n = 0; n < this.mBodies.Count; n++)
            {
                this.mBodies[n].dampenVelocity();
            }
        }

        private void updateBodyBitmask(Body body)
        {
            AABB aabb = body.getAABB();
            int num = (int) Math.Floor((double) ((aabb.Min.X - this.mWorldLimits.Min.X) / this.mWorldGridStep.X));
            int num2 = (int) Math.Floor((double) ((aabb.Max.X - this.mWorldLimits.Min.X) / this.mWorldGridStep.X));
            if (num < 0)
            {
                num = 0;
            }
            else if (num > 0x20)
            {
                num = 0x20;
            }
            if (num2 < 0)
            {
                num2 = 0;
            }
            else if (num2 > 0x20)
            {
                num2 = 0x20;
            }
            int num3 = (int) Math.Floor((double) ((aabb.Min.Y - this.mWorldLimits.Min.Y) / this.mWorldGridStep.Y));
            int num4 = (int) Math.Floor((double) ((aabb.Max.Y - this.mWorldLimits.Min.Y) / this.mWorldGridStep.Y));
            if (num3 < 0)
            {
                num3 = 0;
            }
            else if (num3 > 0x20)
            {
                num3 = 0x20;
            }
            if (num4 < 0)
            {
                num4 = 0;
            }
            else if (num4 > 0x20)
            {
                num4 = 0x20;
            }
            body.mBitMaskX.clear();
            for (int i = num; i <= num2; i++)
            {
                body.mBitMaskX.setOn(i);
            }
            body.mBitMaskY.clear();
            for (int j = num3; j <= num4; j++)
            {
                body.mBitMaskY.setOn(j);
            }
        }

        public int MaterialCount
        {
            get
            {
                return this.mMaterialCount;
            }
        }

        public int PenetrationCount
        {
            get
            {
                return this.mPenetrationCount;
            }
        }

        public float PenetrationThreshold
        {
            get
            {
                return this.mPenetrationThreshold;
            }
            set
            {
                this.mPenetrationThreshold = value;
            }
        }

        [StructLayout(LayoutKind.Sequential)]
        private struct BodyCollisionInfo
        {
            public Body bodyA;
            public int bodyApm;
            public Body bodyB;
            public int bodyBpmA;
            public int bodyBpmB;
            public Vector2 hitPt;
            public float edgeD;
            public Vector2 normal;
            public float penetration;
            public void Clear()
            {
                this.bodyA = (Body) (this.bodyB = null);
                this.bodyApm = this.bodyBpmA = this.bodyBpmB = -1;
                this.hitPt = Vector2.Zero;
                this.edgeD = 0f;
                this.normal = Vector2.Zero;
                this.penetration = 0f;
            }
        }

        public delegate bool collisionFilter(Body bodyA, int bodyApm, Body bodyB, int bodyBpm1, int bodyBpm2, Vector2 hitPt, float normalVel);

        [StructLayout(LayoutKind.Sequential)]
        public struct MaterialPair
        {
            public bool Collide;
            public float Elasticity;
            public float Friction;
            public World.collisionFilter CollisionFilter;
        }
    }
}

